from socket import *
from matplotlib import pyplot as plt
import sys, os, struct
import math
from threading import Thread, Lock
from time import sleep
import numpy as np
from matplotlib.widgets import Button
import sklearn
import scipy.signal as signal
from scipy.interpolate import lagrange

sys.path.append(os.path.join(sys.path[0], '../..'))

from csi_data import CSI_Data
from csi_config import read_setting

PORT = int(read_setting('port'))

BUF_SIZE = 2048
client = socket(AF_INET, SOCK_STREAM, 0)
client.connect(('127.0.0.1', PORT))
totalcnt = 0


def getmodl(arr):
    return [math.sqrt(item.real ** 2 + item.imag ** 2) for item in arr]


def getmod(item):
    # return math.sqrt(item.real ** 2 + item.imag ** 2)
    return np.abs(item)


def getphase(item):
    return np.angle(item)


def read_t():
    alive = True
    while alive:
        try:
            buf = client.recv(BUF_SIZE)
            if buf is None:
                client.close()
                sys.exit(-2)
            i = struct.unpack('>H', buf[:2])
            if i[0] == len(buf) - 2:  # correct package check - 1
                global totalcnt
                totalcnt += 1

                if buf[2] == 187:  # correct package check - 2
                    dat = CSI_Data(buf[3:])
                    csi_process_phase(dat)  # 观察相位差
        except struct.error as e:
            alive = False
            print(e)
            client.close()


# csi数据处理--两个天线30个子载波的相位差
def csi_process_phase(dat):
    subcarrier = 0
    subcarrier2 = 29
    if dat is not None:
        print(dat.correct, dat.Ntx, dat.Nrx)
        if dat.Nrx == 1:  # 防止出现数据接收错误
            return
        csi_t = [0] * 30
        for i in range(30):
            csi1 = getphase(dat.csi[0][0][i])
            csi2 = getphase(dat.csi[0][1][i])
            if csi1 >= 0 and csi2 >= 0:
                if csi1 >= csi2:
                    temp = csi1 - csi2
                else:
                    temp = csi2 - csi1
            elif csi1 > 0 and csi2 < 0:
                t = csi1 - np.pi
                if csi2 >= t:
                    temp = np.pi-csi2 + t
                else:
                    temp = np.pi * 2 + csi2 - csi1
            elif csi1 < 0 and csi2 > 0:
                t = csi2 - np.pi
                if csi1 >= t:
                    temp = np.pi-csi1 + t
                else:
                    temp = np.pi * 2 + csi1 - csi2
            else:
                if csi1 >= csi2:
                    temp = csi1 - csi2
                else:
                    temp = csi2 - csi1
            # temp=csi1-csi2
            # if temp<=-np.pi:
            #     temp+=np.pi*2
            # elif temp>=np.pi:
            #     temp-=np.pi*2
            csi_t[i] = temp

        # 得到30个子载波的相位差

        # fil.append(csi_t[subcarrier])
        # fil.remove(fil[0])
        # if abs(fil[-1])>3:
        #     fil[-1] = 2*fil[-2]-fil[-3]
        # csi_fil = np.average(fil)

        csi_y.reverse()
        csi_y.append(csi_t[subcarrier])
        csi_y.remove(csi_y[0])
        csi_y.reverse()

        # sf=calman(csi_y[:])

        line[0].set_ydata(csi_y)
        csi_y2.reverse()
        csi_y2.append(csi_t[subcarrier2])
        csi_y2.remove(csi_y2[0])
        csi_y2.reverse()

        # sf=calman(csi_y[:])

        line2[0].set_ydata(csi_y2)
        plt.draw()


# 自定义差值函数
def interp(s, n, k=5):
    y = s[list(range(n - k), n) + list(n + 1, n + k + 1)]
    y = y[y.notnull()]
    return lagrange(y.index, list(y))(n)


# 自定义卡尔曼滤波
def calman(s):
    z = np.mat(s)
    global p_local
    x_mat = np.mat([s[1]], )  # 定义初始状态
    p_mat = p_local  # 定义初始状态协方差矩阵
    f_mat = np.mat([1, ])  # 定义状态转移矩阵
    q_mat = np.mat([1, ])  # 定义状态转移矩阵协方差矩阵
    h_mat = np.mat([1, ])  # 定义观测矩阵
    r_mat = np.mat([0])  # 定义观测噪声协方差矩阵

    x_predict = f_mat * x_mat
    p_predict = f_mat * p_mat * f_mat.T + q_mat
    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
    x_mat = x_predict + kalman * (z[0, 0] - h_mat * x_predict)
    p_mat = (np.eye(1) - kalman * h_mat) * p_predict
    s[0] = x_mat[0, 0]

    p_local = p_mat

    return s


plt.figure(1)
plt.ylim(-10, 10)

note = [0] * 3
fil = [0] * 10

p_local = np.mat([1, ])  # 定义初始状态协方差矩阵

num = 200
csi_y = [0] * num
csi_y2 = [0] * num
plt.subplot(211)
plt.ylim(-1, 5)
line = plt.plot(range(0, num), [0 for _ in range(num)], 'r')  # 经数据处理之后的图线
plt.subplot(212)
plt.ylim(-1, 5)
line2 = plt.plot(range(0, num), [0 for _ in range(num)], 'g')
read_thread = Thread(target=read_t)

read_thread.start()
plt.show()
read_thread.join()
